#include "PrescribedMotion.h"
#include "Joint.h"
#include "Index.h"
#include "ABAData.h"
#include "RigidBodyNode.h"
#include "Force.h"
#include "FastOps.h"

#include <iostream>
#include <cstdlib>
#include <cmath>
#include <assert.h>

using namespace std;
using namespace RSIM;

///////////////////////////////////////////////////////

void Joint::setQFromPrescribedMotion(State& state){
        if(this->isMotionPrescribedAndActive_){		
                PM_->setQ(state.getSysData(), state.wgetQForNode(NodeIndex_) );
        }
}

///////////////////////////////////////////////////////

void Joint::setUFromPrescribedMotion(State& state){
        if(this->isMotionPrescribedAndActive_)
                PM_->setU(state.getSysData(), state.wgetUForNode(NodeIndex_));
}

///////////////////////////////////////////////////////

void Joint::stopPrescribedMotion(){
        if(this->PM_){
                this->isMotionPrescribedAndActive_ = false;
        }
        else{
                cout<<"\nvoid Joint::stopPrescribedMotion(): Motion not prescribed for this joint. Must be added "<<
                "using MultibodySystem::addPrescribedMotion(const BodyIndex&, PrescribedMotion*)\n ";
                exit(0);
        }
}

///////////////////////////////////////////////////////

void Joint::continuePrescribedMotion(){
        if(this->PM_)
                this->isMotionPrescribedAndActive_ = true;
        else{
                cout<<"\nvoid Joint::continuePrescribedMotion(): Motion not prescribed for this joint. Must be added " 
                <<"using MultibodySystem::addPrescribedMotion(const BodyIndex&, PrescribedMotion*)\n ";
                exit(0);
        }
        
}

///////////////////////////////////////////////////////

Joint::Joint(const JointType& JT):JointUtils(),T_MF_(),PM_(NULL),
                                isMotionPrescribedAndActive_(false){
}

///////////////////////////////////////////////////////

Joint::~Joint(){
        delete PM_;
}

///////////////////////////////////////////////////////

void Joint::updOneQ(State& state, const QIndex& qindex, const double& q){
        
        if(isMotionPrescribedAndActive_){
                #ifdef RSIM_VERBOSE
                        cout<<"\nMotion is already prescribed on this joint. Ignored call to"
                        <<"Joint::updOneQ(const QIndex& qindex, const double& q)\n";
                #endif
        }
        else{
                state.wgetQForNode(NodeIndex_)[qindex.id()]=q;
        }
}

///////////////////////////////////////////////////////

void Joint::updOneU(State& state, const UIndex& uindex, const double& u){
        
        if(isMotionPrescribedAndActive_){
                #ifdef RSIM_VERBOSE
                        cout<<"\nMotion is already prescribed on this joint. Ignored call to"
                        <<"Joint::updOneU(const UIndex& uindex, const double& u)\n";
                #endif
        }
        else{
                state.wgetUForNode(NodeIndex_)[uindex.id()]=u;
        }
}

///////////////////////////////////////////////////////

const double Joint::getOneU(const State& state, const UIndex& uindex) const {
        #ifdef RSIM_SAFE
                assert(uindex.id() >= 0);
                assert(uindex.id() < state.getBodyNU(NodeIndex_));
        #endif
        return state.getUForNode(NodeIndex_)[uindex.id()];
}

///////////////////////////////////////////////////////

const double Joint::getOneQ(const State& state, const QIndex& qindex) const {
        #ifdef RSIM_SAFE
                assert(qindex.id() >= 0);
                assert(qindex.id() < state.getBodyNQ(NodeIndex_));
        #endif

        return state.getQForNode(NodeIndex_)[qindex.id()];
}

///////////////////////////////////////////////////////